#include <rtthread.h>
#include "rplidar.h"

#define RPLIDAR_DEVICE_NAME    "rplidar"    /* 设备名称 */

static int rplidar_scan_example(int argc, char *argv[])
{
    rt_err_t ret;
    char buf1[100];
    char buf2[100];
    // 1. 获取激光雷达设备
    rt_device_t lidar = rp_lidar_create(RPLIDAR_DEVICE_NAME);
    if (lidar == RT_NULL)
    {
        rt_kprintf("Error: RPLIDAR device not found!\n");
        return -RT_ERROR;
    }

    // 2. 初始化雷达设备
    ret = rp_lidar_init(lidar);
    if (ret != RT_EOK)
    {
        rt_kprintf("Error: RPLIDAR init failed!\n");
        return -RT_ERROR;
    }

    // 3. 启动扫描模式
    ret = rp_lidar_scan(lidar, 1000); // 超时1秒
    if (ret != RT_EOK)
    {
        rt_kprintf("Error: Start scan failed!\n");
        return -RT_ERROR;
    }
    rt_kprintf("RPLIDAR scanning started...\n");

    // 4. 循环读取扫描数据
    while (1)
    {
        rplidar_response_measurement_node_t node;

        // 获取单次扫描数据节点
        ret = rp_lidar_get_scan_data(lidar, &node, 1000);
        if (ret != RT_EOK)
        {
            rt_kprintf("Warning: Scan data timeout\n");
            continue;
        }

        // 5. 解析角度和距离（关键修改点）
        float angle = (node.angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / 64.0f;
        float distance = node.distance_q2 / 4.0f;

        // 打印有效数据（距离>0时有效）
        if (distance > 0)
        {
            sprintf(buf1,"%3.2f",angle);
            sprintf(buf2,"%4.2f",distance);
            rt_kprintf("Angle: %s°  Distance: %s mm\n",
                      buf1,
                      buf2);
        }

        rt_thread_mdelay(50); // 适当延时防止CPU占用过高
    }

    return RT_EOK;
}

MSH_CMD_EXPORT(rplidar_scan_example, rplidar continuous scan example);
